/*
 *  arctan.c
 *  
 *
 *  Created by Glen Rhodes on 4/3/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

 #include "arctan.h"



unsigned short arctan_inputs[91] = {
0 ,
17 ,
34 ,
52 ,
69 ,
87 ,
104 ,
121 ,
140 ,
158 ,
175 ,
193 ,
211 ,
229 ,
248 ,
267 ,
286 ,
305 ,
324 ,
343 ,
364 ,
383 ,
403 ,
423 ,
444 ,
465 ,
487 ,
508 ,
531 ,
553 ,
577 ,
600 ,
623 ,
649 ,
674 ,
699 ,
725 ,
753 ,
780 ,
809 ,
838 ,
870 ,
900 ,
931 ,
965 ,
1000 ,
1036 ,
1073 ,
1110 ,
1149 ,
1193 ,
1235 ,
1281 ,
1327 ,
1378 ,
1429 ,
1483 ,
1540 ,
1603 ,
1664 ,
1732 ,
1805 ,
1880 ,
1966 ,
2050 ,
2146 ,
2248 ,
2358 ,
2478 ,
2606 ,
2745 ,
2907 ,
3077 ,
3273 ,
3494 ,
3740 ,
4024 ,
4348 ,
4724 ,
5163 ,
5687 ,
6326 ,
7122 ,
8198 ,
9557 ,
11448 ,
14449 ,
19192 ,
29382 ,
58764,
65000 };

unsigned short arctan_output[91] = {
1 ,
97 ,
194 ,
298 ,
395 ,
499 ,
597 ,
695 ,
799 ,
898 ,
997 ,
1096 ,
1195 ,
1295 ,
1395 ,
1496 ,
1596 ,
1698 ,
1800 ,
1897 ,
2001 ,
2099 ,
2197 ,
2297 ,
2397 ,
2497 ,
2600 ,
2694 ,
2800 ,
2897 ,
3000 ,
3100 ,
3195 ,
3299 ,
3399 ,
3497 ,
3596 ,
3698 ,
3797 ,
3899 ,
3996 ,
4102 ,
4199 ,
4297 ,
4398 ,
4500 ,
4601 ,
4702 ,
4800 ,
4897 ,
5003 ,
5100 ,
5202 ,
5301 ,
5403 ,
5502 ,
5600 ,
5700 ,
5804 ,
5899 ,
5999 ,
6102 ,
6199 ,
6305 ,
6399 ,
6502 ,
6602 ,
6702 ,
6802 ,
6900 ,
6998 ,
7102 ,
7199 ,
7301 ,
7403 ,
7503 ,
7604 ,
7704 ,
7804 ,
7903 ,
8002 ,
8101 ,
8200 ,
8304 ,
8402 ,
8500 ,
8604 ,
8701 ,
8805 ,
8902,
9000};

signed short tilt_angle(signed short x, signed short y){

  unsigned short arctan_in;
  unsigned short x_abs,y_abs;
  unsigned short tilt;
  unsigned short y_slope,x_slope,x_diff;
  char quadrant;
  char i;


  if(x >= 0 && y>=0){
     quadrant = 1;
    x_abs = x;
    y_abs = y;
  }else if(x< 0 && y >=0){
    quadrant = 2;
    x_abs = -x;
    y_abs = y;
  }else if (x<0 && y<0){
    quadrant= 3;
    x_abs = -x;
    y_abs = -y;
  }else if (x>=0 && y<0) {
    quadrant= 4;
    x_abs = x;
    y_abs = -y;
  }


  arctan_in = ((unsigned long)x_abs*1000)/y_abs;


  for(i = 0; i <= 90;i++){
   if(arctan_in < arctan_inputs[i])  {
      i--;
      break;
   }
  }
  if(i<90){
   x_diff =arctan_in - arctan_inputs[i];
   y_slope = arctan_output[i+1] - arctan_output[i];
    x_slope = arctan_inputs[i+1] - arctan_inputs[i];
    tilt = arctan_output[i]+ (x_diff * y_slope)/x_slope;
  }else {
 
  }

  if(y_abs== 0)
      tilt = 9000;
  switch(quadrant){
    case 1: tilt = tilt;break;
    case 2: tilt = - tilt;break;
    case 3: tilt = tilt;break;
    case 4: tilt =  - tilt;break;
  }  

  return tilt;
}